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Abstract 


This  report  describes  the  state  space  model  which  forms  the  core  technology  of 
an  integrated  autonomous  navigation  system  incorporating  perception,  control, 
and  position  estimation  called  RANGER.  The  high  speed  local  navigation  prob¬ 
lem  is  formulated  as  an  optimal  control  problem  in  state  space.  This  report  con¬ 
centrates  on  the  trajectory  tracking,  obstacle  avoidance  and  sensor  stabilization, 
aspects  of  the  system.  These  algorithms  form  the  basis  of  RANGER’S  Controller 
object 
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1.  Introduction 


RANGER  is  an  acronym  for  Real-time  Autonomous  Navigator  with  a  Geometric  Engine.  This 
report  describes  the  “geometric  engine”  part  of  RANGER  -  the  state  space  model,  and  the 
associated  control  algorithms. 

A  new  approach  to  the  high  speed  autonomy  problem  is  presented  which  is  based  fundamentally 
on  the  state  space  representation  of  a  multi-input  multi-output  linear  system  and  the  problem 
analysis  presented  in  [28].  The  system  closes  the  overall  perceive-think-act  loop  for  a  robot  vehicle 
at  relatively  high  update  bandwidth  and  incorporates  both  somatic  and  environmental  feedback. 

A  high  fidelity  feedforward  actuator  dynamics  and  terrain  following  model  is  introduced  which 
formulates  the  local  navigation  problem  as  an  optimal  control  problem.  Feedforward  solves  the 
historically  significant  clothoid  generation  problem  trivially.  C  space  combinatoric  explosion  is 
solved  by  planning  in  actuation  space.  The  system  feeds  steering  commands  to  the  state  space 
model,  and  it  uses  the  terrain  map  to  generate  the  near  clothoid  response  of  the  vehicle  naturally 
and  directly  with  none  of  the  algorithmic  sensitivities  of  classical  solutions.  Obstacle  avoidance, 
path  planning,  path  generation,  and  path  following  algorithms  are  introduced  which  are  based  on 
the  state  space  model.  These  algorithms  are  stable  and  reliable  at  10  mph  on  rough  terrain  and 
promise  to  maintain  stability  at  20  mph  and  above. 

The  system  is  concerned  with  the  high  level  coordinated  control  problem.  It  is  not  concerned  with 
the  specific  control  of  actuators  themselves.  Further,  while  it  can  accept  a  specification  of  a 
strategic  goal,  such  as  a  path  to  follow,  or  a  direction  to  prefer,  it  carmot  generate  its  own  strategic 
goals.  Therefore,  it  solves  the  local  planning  problem  for  autonomous  vehicles.  The  local  planning 
problem  includes  obstacle  avoidance,  and  goal  seeking. 

The  system  caifbe  classified  as  an  inteUigent,  predictive  controller  for  autonomous  vehicles.  It  is 
intelligent  because  it  uses  range  images  that  are  generated  from  either  a  laser  rangefinder  or  a  stereo 
triangulation  rangefinder  in  order  to  perceive  the  immediate  environment  of  the  vehicle.  It  is 
predictive  because  it  reasons  based  on  its  knowledge  of  its  own  capability  to  react  to  hazards  in  real 
time  as  well  as  the  transfer  function  of  the  vehicle  which  relates  motion  commands  to  the  response 
to  those  commands.  This  transfer  function  is  concerned  with  the  geometry,  or  kinetics,  of  vehicle 
motion.  There  is  no  explicit  representation  of  force  in  the  system,  so  it  is  basically  a  geometric  one. 

The  system  is  a  state  space  controller  because  it  explicitly  forms  an  expression  of  the  vehicle 
dynamic  state  vector  in  order  to  predict  the  hazard  signals  upon  which  decisions  are  based.  The 
process  which  converts  terrain  shape  and  command  inputs  into  the  associated  response  of  the 
vehicle  is  a  constrained  multidimensional  differential  equation. 

Although  the  word  ‘reasons’  is  used  above,  the  system  takes  a  traditional  control  systems  view 
rather  than  an  artificial  intelligence  view  of  the  autonomous  navigation  problem.  The  path  planning 
problem  is  regarded  as  trivial  due  to  the  nonholonomic  constraint  of  Ackerman  steering.  The 
reason  for  this  is  that  the  region  in  space  that  the  vehicle  can  physically  reach  which  is  also  within 
the  usable  field  of  view  of  a  sensor  decreases  very  rapidly  in  size  as  the  speed  increases. 

As  of  this  writing,  the  system  has  achieved  IS  Km  excursions,  average  speeds  of  IS  Km/hr  and 
intermittent  speeds  of  20  Km/hr  and  all  of  these  achievements  are  unprecedented.  It  owes  it  success 
to  extensive  analysis  of  the  problem  it  is  intended  to  solve. 
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LI  High  Atttonftmy 

One  of  the  unavoidable  realities  of  safe,  high  speed  autonomy,  is  that  any  vehicle  requires 
significant  time  and  space  to  react  to  external  events.  Mth  vehicle  and  computational  latencies 
largely  fixed,  the  only  practical  avenue  available  is  to  actively  search  for  hazards  at  increasing 
distance  from  the  vehicle.  This  implies  inaeased  demands  on  sensor  resolution  and  overall  system 
accuracy,  and  increased  sensitivity  of  planning  decisions  to  small  model  and  sensory  errors  because 
the  system  cannot  wait  until  geometry  is  more  favorable  in  its  assessment  of  hazards.  A  real-time 
autonomous  navigator  actively  manages  its  field  of  regard  instead  of  its  reaction  time  because  the 
latter  cannot  be  feasibly  altered  enough  to  make  a  difference. 

Unavoidable  system  latencies  and  limited  maneuverability  take  on  a  new  level  of  significance.  A 
high  speed  vehicle  is  committed  to  travelling  a  certain  trajectory  for  a  significant  distance  before 
any  decisions  made  in  the  planner  can  be  enacted  in  hardware.  Further,  dynamics  and  actuator 
response  limit  the  ability  of  the  vehicle  to  significantly  alter  its  trajectory  after  the  actuator 
commands  are  received.  These  hard  physical  limits  on  bra^g  and  steering  imply  that  there  is  little 
point  wasting  computational  bandwidth  assessing  hazards  in  regions  where  the  vehicle  is  either 
already  committed  to  go  or  where  it  physically  caimot  go.  The  size  of  the  region  that  the  system 
truly  has  an  option  of  traversing  rapidly  decreases  in  size  as  speeds  increase.  This  of  course  implies 
a  reduced  ability  to  avoid  obstacles,  but  that  is  a  fundamental  physical  cost  of  higher  speeds. 

Planning  algorithms  which  directly  incorporate  knowledge  of  such  dynamics  enjoy  significant 
performance  improvements.  A  control  systems  view  of  a  high  speed  vehicle  lea^  to  complete 
elimination  of  the  historically  significant  problems  of  C  space  combinatoric  explosion  and 
nonholonomically  constrained  path  generation.  The  problem  remains  one  of  search  but  physical 
dynamics  amounts  to  an  overwhelming  constraint  Of  all  possible  trajectories  in  C  space,  only  a 
relative  handful  are  both  dynamically  feasible  and  spatially  distinct  enough  to  warrant 
consideration. 

L2  Commentary 

The  traditional  hierarchical  view  of  the  problem  arises  periiaps  from  the  axioms  of  structured 
programming  from  software  engineering  and  the  artificial  intelligence  view  of  hierarchical 
planning.  In  this  view,  plaimers  generate  command  inputs  to  controllers  which  do  their  best  to 
follow  them.  While  these  are  excellent  tools,  there  is  a  point  at  which  hierarchy  becomes 
counterproductive  when  it  is  mapped  onto  contemporary  computers  and  contemporary  vehicles.  In 
a  real  time  context,  response  time  and  throughput  requirements  are  absolute  metrics  of  success  and 
tightly  coupled  embedded  systems  are  the  traditional  practical  solution  to  such  problems. 

A  basic  assumption  of  the  hierarchical  view  is  that  controllers  are  empowered  to  execute 
commands  given  to  them.  For  the  problem  solved  here,  this  is  not  the  case.  Indeed,  there  is  very 
little  choice  available  to  a  vehicle  controller  about  what  the  vehicle  will  do  over  the  short  term,  and 
any  plaimer  needs  to  incorporate  this  knowledge  into  its  model  of  the  vehicle-enviromnent 
interaction. 

When  the  point  is  reached  when  plaimers  must  incorporate  explicit  high  fidelity  feedforward 
models  of  vehicle  dynamics  and  system  latencies,  the  traditional  hierarchy  has,  in  fact,  been  broken 
because  the  planner  has  become  a  feedforward  controller.  The  question  becomes  not  what  will  / 
tell  the  vehicle  to  do,  but  rather,  how  will  it  respond  to  this  request.  A  HMMWV  is  a  massive 
system  and  its  actuators  can  only  generate  propulsive  and  steering  forces  which  are  a  very  small 
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fraction  of  the  total  inertial  force  on  the  vehicle.  Hence,  the  vehicle  goes  where  it  wills  and  a 
hierarchical  planner-controller  approach  to  the  problem  of  controlling  a  HMMWV  at  high  speed  is 
an  indirect  attempt  to  defy  physics. 

The  only  model  of  a  high  speed  conventional  vehicle  of  practical  validity  in  autonomy  is  a  coupled, 
constrained,  nonlinear,  multidimensional  differential  equation.  Idealized  kinematic  models  of 
vehicle  steering  are  a  major  cause  of  the  brittleness  of  kinematic  arc  based  planners  which  has  not 
been  generally  recognized. 
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The  admissability  modules  evolved  from  earlier  versions  that  were  designed  and  constructed  by  R 
Coulter  and  Tony  Stentz,  and  later  refined  by  George  Mueller.  Earlier  still,  the  Hughes  ALV 
pioneered  the  field  itself  with  a  similar  approach. 

This  system  can  be  considered  to  be  a  natural  evolution  of  the  ideas  of  Tony  Stentz  which  formed 
the  basis  of  the  first  full  geometry  off  road  navigator  at  CMU  since  the  system  implemented  earlier 
by  Martial  Hebert.  This  software  system  was  implemented  by  Barry  Brumitt,  R  Coulter,  A1  Kelly, 
Bill  Burky  and  George  Mueller  under  the  direction  of  Tony  Stentz. 

The  only  reason  that  the  system  discussed  here  could  have  been  conceived  at  all  was  the 
groundbreaking  work  of  this  team  in  boldly  attempting  the  impossible  and  along  the  way 
identifying  the  issues  which  affect  high  speed  performance.  Portions  of  the  original  code  still 
survive  in  modified  form.  The  system  remains  consistent  with  Tony’s  original  concept  of  a  high 
speed  off  road  navigator  based  on  high  fidelity  physical  models. 

Larry  Mathies  and  Todd  Litwin  of  the  Jet  Propulsion  Labs  provided  the  opportunity  to  integrate  the 
system  with  a  wide  field  of  view  stereo  ranging  system  which  provided  an  excellent  forcing 
function  for  the  software  engineering  of  the  code  and  opportunities  to  engineer  the  system  to  accept 
a  generalized  suite  of  environmental  senscr  inputs. 

Larry  educated  me  on  aspects  of  the  physics  of  remote  sensing  and  his  stereo  algorithm.  Todd 
collaborated  with  me  on  the  software  engineering  of  the  code.  Indirectly,  Don  Gennery  told  me  the 
relationship  between  least  squares  suspension  models  and  the  minimum  potential  energy  principle. 
Tam  Nguyen  explained  the  JPL  propulsion  controller  to  me. 
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U  Difffirept^isitoin  Ii 


tant  Historical  Systems 


RANGER  differs  from  all  historical  precedents  known  to  the  author  in  two  ways.  First,  the  system 
explicitly  evaluates  vehicle  safety  in  terms  of  timing,  speed,  resolution,  and  accuracy  in  real-time 
and  employs  many  adaptive  mechanisms  which  optimize  performance.  In  particular,  explicit 
computation  of  stopping  distance  and  required  minimum  throughput  is  performed  and  these 
considerations  and  others  form  the  basis  of  an  elegant  adaptive  perception  mechanism  which 
makes  unprecedented  vehicle  speeds  possible.  Second,  the  system  uses  relatively  high  fidelity 
feedforward  models  of  system  dynamics  and  terrain  following  a:id  is  configured  as  a  tightly 
coupled  controller. 


This  section  summarizes  the  capabilities  of  some  of  the  most  significant  historical  off  road  systems. 
The  list  is  not  exhaustive,  nor  is  it  a  complete  discussion  of  each  system  provided.  Unfortunately, 
little  has  been  published  to  date  on  some  systems,  and  available  publications  do  not  address  some 
of  the  issues  investigated  here. 


1.4.1  Hughes  ALV 

The  Hughes  Autonomous  Land  Vehicle  [9]  was  one  of  the  first  off-road  mobile  robots  ever 
developed.  The  system  generalized  the  notion  of  an  obstacle  to  include  any  vehicle  configuration 
that  is  unsafe.  This  allowed  navigation  over  terrain  of  any  surface  shape.  Based  on  the  ERIM 
rangefinder  and  a  highly  terrainable  vehicle,  it  achieved  speeds  up  to  3.5  km/hr,  over  distances 
exceeding  500  meters,  and  running  times  averaging  20  minutes. 

The  speed  of  this  vehicle  was  limited  by  many  factors  including  the  complexity  of  the  perception 
and  planning  processing,  and  the  speed  of  communication  with  off  board  processing.  Robusmess 
was  limited  by  low  fidelity  modelling  of  steering  constraints,  poor  terrainability  and 
maneuverability  of  the  vehicle  on  wet  muddy  soil,  inability  to  detect  small,  but  dangerous  obstacles 
(steel  fence  spikes),  constrained  excursion  due  to  line  of  sight  radio  communications,  local 
planning  minima  and  bugs  in  the  high  level  planner. 

The  system  discussed  here  differs  from  the  ALV  in  its  real-time  minimalist  approach  to  the 
problem.  The  adaptive  perception  algorithms  ate  considerably  faster  than  the  Connection  Machine 
implementation  of  the  ALV  algorithms.  It  also  discards  the  costly  energy  minimization 
interpolation  scheme  of  the  ALV  in  favor  of  a  temporal  interpolation  of  the  hazard  vector  signal. 
The  system  also  differs  in  its  high  speed  design  incorporating  a  differential  equation  state  space 
model. 


1.4.2  JPL’sRobby 

The  JPL  rover  [46]  was  the  first  system  to  drive  autonomously  across  outdoor  terrain  using  dense 
stereo  data,  rather  than  sonar  or  laser  rangefinder  data.  This  system  was  demonstrated  in  a  100 
meter  cross-country  demo  in  Sept.  1990.  It  has  achieved  an  average  speed  of  100  meters  in  4 
hours  (7.0  mm/sec)  in  a  start/stop  mode.  The  total  elapsed  time  in  this  case  is  not  very  meaningful, 
since  it  includes  downtime  while  the  system  was  being  debugged  and  numerous  stops  to  evaluate 
performance  ^  The  vehicle  speed  was  mechanically  limited  to  4  cm/sec  and  off  board 
communication  throughput  was  limited.  Cycle  times  were  on  the  order  of  30  secs  for  planning  and 
10  secs  for  perception.  Passive  stereo  was  used  in  the  perception  system.  Later  runs  were  able  to 
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achieve  speeds  of  4  cm/sec. 

The  system  discussed  here  differs  from  Robby  in  that  it  attempts  both  wide  excursion  and  high 
speed.  It  is  difficult  to  assess  the  degree  to  which  computer  evolution  alone  has  made  this  possible. 

1.4.3  JPL’s  Rocky  Series 

The  Rocky  series  of  Microrovers  [43]  are  small  scale  prototypes  intended  as  testbeds  for  simple 
behavior  based  control  systems.  They  are  intended  to  produce  a  flight  rover  for  the  MESUR 
pathfinder  mission  of  1996.  These  robots  are  tested  in  the  context  of  operations  in  the  vicinity  of  a 
planetary  lander,  so  large  traverse  distance  or  high  speed  are  not  being  pursued. 

The  system  discussed  here  differs  from  the  Rocky  series  in  its  deliberative  approach  to  autonomy 
and  its  attempt  to  achieve  wide  excursions  at  higher  speeds.  It  borrows  the  minimalist  configuration 
principle  in  an  attempt  to  optimize  teal  time  performance. 

1.4.4  CMU’s  FastNav 

The  FastNav  project  concentrated  on  achieving  high  vehicle  speeds  and  robust  obstacle  detection 
on  fairly  benign  terrain.  The  Cyclone  single  scanline  laser  rangefinder  was  used  as  the  basis  of  the 
perception  subsystem.  It  was  used  to  check  a  global  pat'  for  discrete  obstacles,  such  as  trees,  rocks, 
and  ditches,  and  stop  accordingly.  Obstacle  detection  was  based  on  detecting  deviations  from  a  flat 
world.  Path  tracking  used  a  position  estimation  system  based  on  inertial  navigation,  dead 
reckoning,  and  the  GPS  satellite  navigation  system. 

FastNav  [41]  made  use  of  simple  tire-soil  interaction  and  vehicle  dynamics  models  to  navigate  on 
locally-flat  terrain  with  sparse  obstacles.  Path  tracking  was  demonstrated  at  30  km/hr  and  obstacle 
detection  was  demonstrated  at  3  m/s. 

Some  of  the  reliability  problems  that  were  demonstrated  include  high  sweep  rates  of  the  Cyclone 
scanner  due  to  vehicle  pitching,  and  poor  accuracy  of  the  inertial  navigation  system.  Early 
problems  with  instability  of  the  tracking  algorithm  were  fixed  by  incorporating  a  first  order  model 
of  the  steering  dynamics  into  a  feedforward  compensator.  The  longest  autonomous  run  was  1 
kilometer^. 

RANGER  borrows  heavily  from  the  FASTNAV  project  in  three  ways.  First,  steering  dynamics 
feedforward  is  generalized  to  a  complete  state  space  rough  terrain  model.  Second,  the  mete  fact 
that  such  speeds  were  possible  pointed  directly  to  a  problem  with  the  way  in  which  nonadaptive 
image  processing  was  a  major  cause  of  poor  real-time  performance.  The  line  scanner  has  been  an 
important  existence  proof  that  the  bare  throughput  requirement  at  these  speeds  was  far  less  severe 
than  it  appears  to  be.  Third,  the  pure  pursuit  paA  tracker  is  an  adaptation  of  Ae  FASTNAV  tracker 
for  rough  terram. 

RANGER  aU^'  differs  from  FASTNAV  m  Aat  it  is  a  cross  country  system,  not  a  dirt  road  system. 
Full  3D  models  are  employed  and  Ae  considerable  difficulties  of  rough  terrain  drive  many  aspects 
of  Ae  design. 


2.  Mucb  of  this  is  personal  communication  with  Sanjiv  Singh  and  Jay  West  Existing  documentation  does  not 
cover  the  issues  mentioned  completely. 


A  Feedforward  Control  Approach  to  the  Local  Navigation  Problem  for  Autonomous  Vdiicles 


pages. 


1.4.5  CMU  UGV  Systems 

From  September  1990  to  the  present,  CMU  reseju’chers  have  developed  two  preliminary  versions 
of  autonomous  cross  country  navigators.  Both  navigators  are  based  on  the  Highly  Mobile  Multi- 
Wheeled  Vehicle  (HMMWV).  The  performance  of  the  both  prototypes  has  been  promising  when 
compared  with  that  of  previous  systems.  Both  systems  are  based  on  the  BRIM  laser  rangefinder. 

The  first  system  [7]  is  similar  in  concept  to  the  ALV.  It  generates  a  complete  geometric  description 
of  the  terrain  in  cartesian  coordinates  by  transforming  successive  range  images.  It  evaluates 
candidate  vehicle  trajectories  by  simulating  the  vehicle  state  forward  in  time  in  order  to  predict 
collisions. 

This  system  was  the  first  to  model  dynamic  constraints  on  vehicle  state  in  path  planning.  Path 
generation  was  based  on  an  alternative  to  the  spanning  arcs  approach  of  the  ALV  for  path 
generation  which  is  theoretically  superior  in  cluttered  environments. 

Reliability  problems  for  this  system  are  associated  with  the  poor  fidelity  of  elevation  maps,  cycle 
time  overruns,  and  path  generation  and  planning  algorithm  failures.  Nevertheless,  this  system  has 
achieved  its  purpose  of  advancing  the  state  of  the  art  and  serving  as  an  experimental  testbed. 

The  most  impressive  achievement  consists  of  a  test  run  of  five  kilometers  comprised  of  some  thirty 
orbits  of  the  same  circular  path  on  substantially  flat  terrain  with  a  total  of  less  than  ten  obstacle 
avoidance  maneuvers.  Speed  has  never  exceeded  3  meters  per  second,  and  only  half  of  this  speed 
has  been  achieved  while  avoiding  obstacles. 

RANGER  borrows  heavily  from  this  system  because  it  is  basically  the  next  prototype  in  this  series. 
It  diflfers  from  its  predecessor  in  its  adaptive  perception  algorithms,  the  efficiency  of  its 
admissability  module,  its  universal  3D  models,  and  it  favors  a  controls  feedforward  approach  over 
the  cluttered  environment  search  based  planner. 

More  recently,  a  second  group  [18]  have  developed  a  system  called  Ganesha  which  processes  range 
images  to  recover  regions  of  constant  terrain  gradient  This  system  has  achieved  runs  over 
distances  of  840  meters,  at  speeds  of  2-3  m/s  while  avoiding  natural  obstacles,  and  it  has  probably 
exceeded  these  results  as  of  this  writing. 

RANGER  differs  from  Ganesha  in  that  it  can  be  considered  to  be  more  of  a  deliberative  system  on 
the  spectrum  of  such  systems  because  it  remembers  both  vehicle  state  and  environmental  state  from 
cycle  to  cycle  to  a  higher  level  of  detail  than  Ganesha  does.  RANGER  is  also  theoretically  superior 
to  Ganesha  on  rough  terrain  at  a  cost  of  the  extra  processing  and  slower  reaction  time  required  to 
navigate  in  a  fully  3D  world  model. 

1.4.6  ALVINN 

ALVINN  is  a  road  following  system  based  on  neural  networks  [39].  Although  it  solves  a  different 
problem,  ALVINN  provides  a  second  precedent  for  the  use  of  feedforward  for  the  purposes  of 
imparting  stability  at  high  speed. 

1.4.7  VITA 

The  Daimler-Benz  group  have  developed  impressive  road  followers  based  on  a  control  systems 
approach  to  road  following.  Although  a  different  problem  is  solved  by  these  systems,  the  control 
approach  has  been  an  important  precedent. 
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Requirements 

The  design  of  the  system  is  based  on  these  top  level  requirements.  They  are  the  highest  level  design 
drivers  that  influence  the  design  to  a  large  extent  Certain  of  the  components  would  be  unjustified 
for  systems  with  other  requirements  such  as,  for  example,  slow,  or  start  stop  vehicles  or 
omnidirectional  vehicles. 

1.5.1  Rough  Terrain 

Rugged,  sparsely  vegetated,  mostly  dry  terrain  is  the  target  environment  within  which  the  vehicle 
must  navigate  safely.  A  flat  world  assumption  along  with  discrete  obstacle  detection  is  not 
considered  valid  in  the  target  terrain. 

1.5.2  Continuous,  High  Speed 

Speeds  which  approach  the  speeds  at  which  humans  can  drive  the  same  terrain  (say,  20  mph)  are 
targetted. 

1.5.3  Ackerman  Steer  Vehicle 

While  the  Ackerman  steering  configuration  is  not  ideal  for  planning  purposes,  most  large  vehicles 
intended  for  human  use  employ  this  configuration,  so  the  restriction  to  this  class  of  vehicles  is  not 
as  limiting  as  it  may  appear.  In  any  case,  the  maneuverability  of  such  vehicles  is  dramatically 
different  from  other  alternatives,  particularly  at  speed,  and  the  report  will  show  that  efficiency 
demands  that  a  special  case  solution  for  this  class  of  vehicles  be  adopted.  Nevertheless  momentum 
and  the  nonholonomic  constraint  limits  maneuverability  of  all  high  speed  vehicles,  so  many  of  the 
results  here  apply  generally. 

1.5.4  Robustness 

The  system  is  intended  for  relatively  long  traverses  of  suitable  terrain.  While  robusmess  is  difficult 
to  quantify,  it  is  the  intention  that  reliability  be  sufficient  for  routine  autonomous  excursions  of  tens 
of  kilometers  over  suitably  chosen  terrain. 
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2.  Analytical  Basis  of  the  Concept 

RANGER  is  an  attempt  to  design  an  optimal  system  from  a  systems  perspective.  A  consistent 
requirements  analysis  was  conducted  which  identifies  the  interrelationships  of  requirements  and 
which  attempts  to  optimize  the  system  performance  as  a  whole.  Consistent  resolution  and  accuracy 
goals  are  imposed  on  the  design  so  that  all  subsystems  are  equally  well  and  equaUy  poorly  able  to 
do  their  respective  jobs.  In  many  cases,  trade-offs  are  managed  in  real  time  in  the  code  itself. 

This  approach  arose  for  historical  reasons.  Earlier  versions  of  the  system  had  no  basis  for  allocating 
resources  to  subproblems  so  all  were  optimized  at  their  own  level.  This  led  to  “near  perfect” 
perception  and  planning  algorithms  which  did  an  excellent  job  of  predicting  collisions  with  hazards 
that  they  were  too  busy  ticking  about  to  do  anything  about 

The  system  eliminates  the  lower  levels  of  control  which  traditionally  attempt  to  follow  a  path 
provided  by  a  higher  level  agent  because  the  problem  is  too  coupled  to  solve  by  hierarchy.  The 
system  works  in  actuation  space  and  incorporates  feedforward.  For  this  reason,  it  is  effectively  a 
controller  and  not  a  planner. 

The  system  that  emeiges  from  these  considerations  is  a  unique  one.  It  is  a  real  time  control  system 
which  operates  on  environmental  and  navigation  sensory  inputs  and  incorporates  a  high  fidelity 
model  of  both  the  plant  and  the  environment.  It  is  tighUy  coupled  and  it  merges  the  traditiomd 
hierarchies  into  one  conceptual  layer  that  closes  the  perceive-think-act  loop  at  high  update 
bandwidth.  The  system  is  based  upon  a  large  body  of  analyses  presented  in  [28].  A  brief  discussion 
of  the  most  significant  aspects  of  the  design  is  presented  below. 

2A  Gwirante^  Safety 

The  system  directly  implements  the  policy  of  guaranteed  safety  by  reasoning  in  real  time  about  the 
four  dimensions  of  safety  (i.e.  timing,  speed,  resolution,  and  accuracy)  and  adapting  its  perception 
and  plaiming  subsystems  to  comply  directly  with  the  need  for  safety. 

2.2  Adaptive  Regard 

Adaptive  Regard  is  a  mechanism  for  ensuring  that  the  system  minimizes  the  spatial  extent  of  the 
region  it  perceives  based  on  vehicle  maneuverability  so  that  speed  is  maximized  without 
compromising  safety.  The  principle  of  adaptive  regard  is  to  scan  for  hazards  only  in  the  obstacle 
detection  zone.  This  is  the  region  of  space  that  the  vehicle  still  has  an  option  of  traversing.  The 
system  implements  adaptive  regard  by  adapting  its  obstacle  avoidance  behavior  to  guarantee  that 
an  impulse  turn  can  always  be  executed  should  an  obstacle  be  detected. 

2.2  Fidelity  Adaptive  Planning 

This  principle  surrounds  hazards  by  a  small  buffer  region  in  actuation  space  in  order  to  compensate 
for  any  inaccuracies  in  the  computations.  It  is  implemented  here  as  Gaussian  filtering  of  the 
steering  vote  vector. 
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2A  Feedforward  Path  Generation 

The  configuration  space  of  the  high  speed  nonholonomic  vehicle  is  degenerate  at  high  speed  and 
the  attendant  clothoid  generation  problem  is  impossible  to  solve  in  practical  terms.  Ifie  “path 
generator”  for  the  system  generates,  not  candidate  paths,  but  candidate  command  signals  ejqtresstd 
in  actuation  space  (in  terms  of  curvature  and  speed  as  a  function  of  time),  and  uses  a  high  fidelity 
simulator  to  solve  the  differential  equations  of  motion  in  order  to  determine  the  exact  response  to 
these  candidate  commands.  This  strategy  has  the  following  advantages: 

•  The  paths  generated  meet  the  mobility  constraints  of  the  vehicle  by  construction,  so  the 
difficult  and  often  impossible  problem  of  conversion  ^m  C  space  to  A  space  is  completely 
avoided.  Instead,  the  reverse  process  of  dead  reckoning  is  used  in  the  simulator.  This  is  a  kind 
of  feedforward. 

•  The  paths  coarsely  span  the  entire  region  of  space  that  the  vehicle  can  reach,  so  '  real 
alternatives  are  missed. 

2tS  Feedforward  State  Space  Controller 

The  system  represents  hazards  as  sampled  time  signals  in  a  kind  of  multidimensional  hazard  space. 
The  essential  obstacle  avoidance  mechanism  is  the  minimization  of  an  optimal  control functional 
in  this  space.  Interpolation  is  performed  in  time  along  the  vehicle  state  vector  rather  than  in  the 
space  of  the  terrain  map. 

M  Continuity  Assumption 

The  assumption  that  important  aspects  of  vehicle  maneuverability  and  system  computational 
performance  will  not  change  significantly  from  one  cycle  to  the  next  is  called  the  continuity 
assumption.  This  assumption  is  used  to  resolve  some  of  circular  issues  related  to  the  coupling 
between  adaptive  perception  and  adaptive  regard. 

2J.  Terrain  Smoothness  Assumption 

The  terrain  smoothness  assumption  is  required  in  order  to  implement  a  terrain  map 
representation  of  the  environment.  It  is  also  used  to  justify  the  interpolation  of  small  unknown 
regions. 

M  Unknown  Hazard  Assumption 

The  unknown  hazard  assumption  involves  the  belief  that  large  unknown  regions  in  a  terrain  map 
are  unnavigable  by  default.  This  key  assumption  permits  the  system  to  operate  robustly  near  laige 
holes  or  near  the  edge  of  a  cliff. 
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3.  Hierarchical  Planning  and  Arbitration 

The  local  ndnimiiin  proUem  is  perhaps  the  most  serious  threat  to  robustness  of  any  local  obstacle 
avoidance  strategy  for  autonomous  navigation.  In  order  to  achieve  robust  navigation,  a  sound 
strategic  plan  that  avoids  these  minima  is  a  solution  that  worics  well  in  practice.  In  order  to  enhance 
robustness,  the  system  supports  the  input  and  continuous  monitoring  of  strategic  goals. 

Strategic  Goal 

The  system  defines  a  strategic  goal  which  instantaneously  may  be  any  one  of: 

•  follow  a  predefined  path 

•  drive  toward  a  predefined  point 

•  maintain  a  fixed  compass  direction 

•  maintain  a  fixed  curvature 

12  Tactical  Goal 

The  tactical  goal  is  to  simultaneously  avoid  all  hazardous  conditions  including: 

•  tipover 

•  body  collision  (high  centering) 

•  wheel  collision 

•  entering  unknown  terrain 

and  extensions  to  other  hazards  ate  simple  to  perform. 

11  Arbitration 

In  situations  where  a  hazard  is  detected  which  lies  in  the  way  of  achieving  the  strategic  goal,  the 
tactical  goal  will  override  the  strategic  one  as  long  as  any  hazard  exists.  The  output  of  the  steering 
arbiter  may  be  very  discontinuous  if  a  hazard  suddenly  appears  or  disappears  and  because  high 
steering  rates  constitute  a  hazard  by  themselves,  the  steering  arbiter  output  is  smoothed  to  remove 
steering  discontinuities  except  when  drastic  action  is  called  for.  That  is,  the  strategic  steering  output 
is  smoothed  whereas  the  tactical  one  is  not 

The  arbitration  scheme  might  be  called  a  prioritked  sort.  The  principles  of  this  technique  are: 

•  Obstacle  avoidance  always  overrides  tracking  if  it  chooses. 

•  Tracking  chooses  the  best  path  which  obstacle  avoidance  allows  it  when  it  is  allowed  control. 

This  approach  has  the  side  effect  that  following  of  an  extended  feature  will  emerge  naturally,  and 
the  system  will  immediately  take  the  opportunity  to  reacquire  the  strategic  goal  if  an  opportunity 
presents  itself.  While  this  describes  the  emergent  behavior,  the  algorithm  itself  is  to: 

•  sort  the  votes  of  the  tactical  plarmer  using  the  corresponding  strategic  votes  as  the  key 

•  choose  the  first  path  in  the  sorted  list  which  satisfies  obstacle  avoidance 

Effectively,  if  obstacle  avoidance  does  not  care  which  direction  to  choose,  the  strategic  plarmer  is 
given  control.  If  it  does  care,  it  will  continually  veto  the  strategic  votes  in  order  of  strategic 
preference  until  a  safe  one  is  presented  to  it.  If  no  safe  path  exists,  a  stop  command  is  issued. 
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4.  Path  Tracking 

The  only  nontrivial  aspect  of  tracking  a  strategic  goal  is  high  fidelity  tracking  of  a  convoluted  path 
over  rough  terrain. 

^  Adaptive  Parc  Pursuit 

The  strategic  controller  incorporates  an  adaptive  path  tracker  which  is  based  on  the  pure  pursuit 
algorithm.  The  pure  pursuit  algorithm  has  been  around  for  some  time  [41].  It  is  basically  a 
proportional  controller  formed  on  the  heading  error  computed  from  the  current  heading  and  the 
heading  derived  from  the  current  vehicle  position  and  a  goal  point  on  the  path.  The  goal  point  is 
computed  by  finding  the  point  on  the  path  which  is  a  predetermined  distance  from  the  current 


Heading  is  measured  at  the  center  of  the  rear  axle.  The  proportional  gain  is  normalized  by  the 
lookahead  distance  L.  This  can  be  viewed  an  adaptive  element  or,  more  simply,  as  a  unit 
conversion  from  heading  error  to  curvature.  Indeed,  the  ratio  is  the  average  curvature 

required  to  reacquire  the  path  at  the  goal  point 

A  limiter  is  used  which  ensures  that  the  curvature  is  limited  at  higher  speeds  to  prevent  rollover. 
Another  limiter  maintains  the  angular  acceleration  below  a  threshold.  These  measures  ensure 
vehicle  safety  and  directly  prevent  instability  at  the  cost  of  an  inability  to  track  high  curvature  paths 
at  high  speed.  However,  vehicle  dynamics  prevent  tracking  high  curvature  paths  at  high  speed 
anyway. 

Laige  tracking  errors  or,  equivalently,  too  short  a  lookahead  distance  or  too  high  a  gain  all  result  in 
servo  instability.  This  is  a  well  known  problem  with  pure  pursuit  which  can  be  addressed  with 
feedforward. 
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A  few  modifications  are  introduced  to  adapt  pure  pursuit  for  rough  terrain.  First,  extremely  large 
tracking  errors  must  be  acceptable  to  the  servo  without  causing  instability.  This  is  managed  by  two 


Instead  of  using  the  current  vehicle  position  as  the  origin  of  the  lookahead  vector,  the  system 
maintains  a  running  estimate  of  the  point  on  the  path  which  is  closest  to  the  current  vehicle 
position.  This  is  done  because  it  is  very  expensive  to  search  the  entire  path  each  iteration.  In  doing 
so,  the  system  is  assuming  that  the  vehicle  will  never  have  a  heading  error  which  exceeds  90 
degrees  for  an  extended  period  of  time.  This  is  a  monotone  arc  leng[th  assumption.  This 
assumption  completely  eliminates  the  overwhelming  computational  cost  of  simpler 
implementations  of  the  algorithm. 

The  lookahead  distance  is  adaptive  to  the  current  tracking  error  •  increasing  as  the  error  increases 
as  indicated  in  the  accompanying  code  fragment.  Tlie  first  while  loop  is  responsible  for  maintaining 
a  running  record  of  the  close  point,  point  2.  It  searches  through  an  arc  length  window  which  adapts 
to  the  path  tracking  error.  As  the  error  gets  larger,  this  loop  will  cause  the  close  point  to  jump  over 
high  curvature  kinks  in  the  path  as  they  become  less  relevant  at  the  resolution  of  the  tracking  error. 

The  second  while  loop  computes  the  goal  point  in  an  identical  maimer.  It  basically  moves  point  3 
forward  until  it  falls  outside  a  circle  centered  at  the  vehicle  whose  radius  is  the  sum  of  the  error 
distance  and  the  nonadaptive  lookahead.  In  this  way,  when  an  obstacle  avoidance  maneuver  causes 
significant  path  error,  die  algorithm  will  search  to  reacquire  the  path  on  the  other  side  of  the 
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obstacle  instead  of  causing  a  return  to  the  front  of  the  obstacle. 

Notice  that  under  normal  circumstances  when  the  vehicle  is  tracking  tl^  path,  the  close  point  is  at 
the  vehicle  position,  the  error  distance  is  zero,  and  the  adaptive  lookahead  is  the  nonadaptive 
lookahead.  In  plain  terms,  the  algorithm  gracefully  degenerates  to  classical  pure  pursuit  when 
obstacle  avoidance  is  not  necessary. 


42  Feedforward  Pure  Pursuit 

An  upcoming  section  will  present  a  high  fidelity  feedforward  simulator  which  models  many 
aspects  of  vehicle  kinetics.  A  feedforward  option  in  the  tracking  algorithm  incorporates  the  output 
of  this  simulator  into  the  tracker.  The  cost  of  a  feedforward  simulator  must  be  borne,  so  an 
additional  feedforward  element  in  the  tracker  is  available  for  free.  The  basic  idea  is  as  follows. 
First,  at  each  point  in  the  simulation,  evaluate  the  distance  from  the  vehicle  to  the  goal  point 
Second,  the  candidate  command  which  comes  closest  to  the  goal  point  becomes  the  vote  of  the 
strategic  controller.  Such  an  algorithm  provides  excellent  path  following  in  three  dimensions  over 
rough  terrain. 


The  concept  is  indicated  in  the  following  figure.  During  a  high  curvature  turn  at  speed,  the 
feedforwaid  simulator  skews  the  response  curves  toward  the  current  steering  direction. _ 


Note,  the  vehicle 
is  in  a  sharp  left  tum 
and  h^)p^  to  have 
zero  beading  at  tM). 


Figure  3  -  Feedforward  Pure  Pursuit 


A  kinematic  tracker  would  issue  a  hard  right  command  in  the  situation  depicted  above  whereas  a 
dynamic  one  would  recognize  that  such  a  command  would  actuaUy  increase  the  tracking  error.  A 
dyiuunic  tracker  would  issue  a  zero  curvature  command  and  would  correctly  acquire  the  goal  point 
as  the  steering  wheel  slowly  turned  toward  zero.  The  following  figure  illustrates  the  performance 
of  the  feedforward  algorithm  relative  to  the  kinematic  one. 
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Figure  4  -  Performance  of  Feedforward  Pure  Pursuit 


Each  graph  shows  the  target  commanded  trajectory  and  the  actual  trajectory  followed  under  the 
algorithm  used.  Both  paths  are  indistinguishable  most  of  the  time.  Note,  however,  that  the 
kinematic  tracker  goes  unstable  in  the  top  left  figure  whereas  the  feedforward  one  does  not 
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5.  Adaptive  Regard 

The  adaptive  regard  principle  can  be  expressed  as  follows:  There  is  no  need  to  process  data  in 
regions  where: 

•  the  vehicle  is  already  committed  to  going  (dead  zone) 

•  the  vehicle  cannot  go  (free  zone) 

•  subsequent  images  will  provide  better  measurements  (beyond  detection  zone) 
because  in  all  cases,  there  is  no  useful  decision  that  a  planner  can  make. 

In  the  first  case,  if  an  obstacle  enters  the  dead  zone,  system  failure  is  assured,  so  they  must  be 
detected  and  avoided  before  they  get  this  close.  In  the  second  case,  processing  such  data  is  a 
complete  waste  of  resources.  In  the  third  case,  processing  can  be  postponed  until  a  future  cycle. 

If  the  vehicle  cannot  avoid  obstacles  at  close  quarters,  not  only  is  there  no  need  for  processing 
image  geometry  there,  there  is  also  no  need  for  a  planner  to  evaluate  safety  in  its  imm^iate  vicinity 
in  the  map.  The  system  plarmer  evaluates  trajectory  safety  only  within  the  planning  window.  The 
algorithm  for  computing  this  window  based  on  an  impul^  turn  maneuver  is  given  below.: 

/* 

**  Plan  Window 
*/ 

Pntzoc  =  speed  *  treact  +  rhomin;/*  adaptive  lookahead  */ 

Pmin  =  Pm2ix  -  iinaging_density  *  speed  ♦  cycle_time  /*  adaptive  sweep  */ 


An  impulse  turn  is  a  turn  from  zero  curvature  to  the  maximum  allowed  curvature.  It  is  the 
maneuver  necessary  to  avoid  a  large  obstacle  without  slowing  or  stopping.  The  imaging  density  is 
the  average  requested  density  of  images  on  the  groundplane.  It  must  be  maintained  above  1.0  at  all 
times  to  ensure  adequate  coverage  of  the  envirorunent  Later,  the  perception  module  will  map  this 
plarming  window  into  image  space  to  allow  it  to  extract  the  requested  data. 


A  Feedfiwwaid  Control  Approach  to  the  Local  Navigation  Problem  for  Autonennous  Vehicles 


page  15. 


6.  Real  Time  Latency  Modelling 

As  a  real  time  system,  a  reliable  measurement  of  time  is  requited  for  the  system  to  function 
correctly.  In  particular,  the  system  needs  to  match  images  with  vehicle  states  which  may  arrive 
intermittently  and  it  needs  to  remember  the  commands  that  were  issued  to  all  actuators  in  previous 
iterations  of  the  main  control  loop  because  the  system  cycles  faster  than  the  output  latencies.  This 
is  accomplished  with  the  following  mechanisms. 

•  all  input  and  ouq)ut  is  time  stamped 

•  all  input  and  ouqjut  is  stored  in  internal  FIFO  queues 

•  all  sensor  latencies  are  modelled 

•  all  actuator  latencies  ate  modelled 

It  is  important  to  recognize  that  these  FIFO  queues  do  not  introduce  artificial  delay.  Their  function 
is  to  model  delay  which  already  exists  in  the  hardware.  Conceptually,  all  i/o  goes  through  these 
queues. 

iul  Localization  of  Image  Pixels 

The  smearing  of  the  environment  model  which  arises  from  incorrect  models  of  the  motion  of  the 
sensor  during  the  image  digitization  process  is  called  the  motion  distortion  problem.  All 
incoming  vehicle  state  information  is  stored  in  a  FIFO  queue  until  it  is  needed  to  process  images 
which  arrive  later.  The  state  queue  and  the  time  tags  are  used  to  effectively  register  the  ray  through 
every  pixel  with  a  unique  vehicle  pose  before  its  value  is  converted  to  world  coordinates. 

Localization  of  the  Vehicle 

While  temporal  registration  of  images  and  vehicle  positions  is  a  resolution  matter,  the  raw  delay  of 
the  vehicle  position  estimate  is  a  separate  accuracy  concern.  When  the  system  uses  the  latest 
vehicle  state  as  its  estimate  of  position,  the  error  involved  in  doing  so  is  die  velocity  times  the 
latency.  Clearly  a  delay  of  only  one  second  causes  5  meters  of  localization  error  in  the  vehicle 
position  at  5  ni/s  and  pretty  well  guarantees  collisions  with  obstacles  solely  because  the  system 
does  not  know  how  close  it  is  to  the  obstacle^.  For  this  reason,  an  estimate  of  position  sensor 
latency  is  used  and  fed  forward  just  like  any  other  parameter^. 

^  Localization  of  the  Feedforward  Vehicle 

All  output  commands  ate  stored  in  a  FIFO  queue  and  ate  used  for  the  next  several  iterations  in 
implementing  command  feedforward.  When  the  system  is  generating  the  current  command,  there 
may  be  quite  a  few  previous  commands  still  en  route  to  the  hardware  so  a  proper  model  must  feed 
these  pending  commands  into  the  model  before  the  current  one  is  used  and  make  decisions  based 
on  the  last  command  to  go  out,  because  the  system  still  has  control  over  only  that  current  command. 


3.  This  is  an  almost  intolerable  situation,  particularly  when  steering  response  is  added  to  the  picture,  and  it 
argues  persuasively  for  tight  system  couptog.  lb  ignore  these  issues  at  5  m/s  it  to  overestimate  obstacle  clear¬ 
ance  by  5  meters,  underestimate  the  distance  requited  to  turn  by  as  much  or  more,  and  drive  straight  into  any¬ 
thing  in  die  way.  However,  the  best  that  can  be  done  is  to  model  the  latency  if  it  cannot  be  reduced. 

Roughly  speaking,  a  2  second  delay  from  image  to  steering  actuator  makes  10  m/s  speed  inqiossible. 

4.  This  is  a  practical  measure  because  extant  vehicle  positioning  systems  and  computing  hardware  have  no 
consistent  time  standard.  Ideally,  the  latency  could  be  simply  measured  from  the  current  time  and  the  time  tag 
of  the  image. 
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7.  Feedforward  Simulation 


In  order  to  evaluate  safety,  the  tactical  control  module  computes  the  response  to  candidate 
commands  simulated  forward  in  time  until  the  detection  zone  is  reached.  The  basic  justification  for 
this  approach  is  an  attempt  to  meet  guaranteed  response  and  localization  simultaneously.  In  order 
to  malre  sound  decisions,  the  system  must  know  now  where  the  vehicle  will  be  many  seconds  later 
if  a  particular  command  is  issued  now.  It  carmot  “wait  until  it  gets  there”  to  measure  its  response 
because  then  it  will  be  too  late  to  react 


The  solution  of  this  problem  is  mathematically  involved  because  it  involves  solution  of  a  coupled 
set  of  eight  nonlinear  differential  equations  which  form  the  vehicle  state  space  model.  The  system 
mechanizes  these  equations  in  real  time.  The  equations  are  coupled  because: 


•  position  at  step  i  depends  on  attitude  and  speed  at  step  i-1 

•  attitude  at  step  i  depends  on  steering  response,  propulsion  response,  and  attitude  step  i-1 

•  steering  response  at  step  i  depends  on  steering  response  and  steering  command  at  step  i-1 

•  propulsion  response  at  step  i  depends  on  attitude,  propulsion  response  and  propulsion 
command  at  step  i-1 

•  attitude  at  step  i  depends  on  suspension  state  at  step  i-1 


2il  State  Space  Terrain  Following  Model 


By  solving  the  equations  in  their  true  coupled  form,  the  system  can  correctly  simulate  a  vehicle 
driving  along  the  side  of  a  hill,  for  example.  It  correctly  simulates  the  dependence  of  actual 
trajectory  on  the  terrain  itself,  the  speed,  the  actuator  response  characteristics,  and  the  initial 
conditions. 


The  basic  simulation  loop  can  be  written  as  follows.  At  each  time  step: 


•  simulate  suspension  -  determine  attitude  from  terrain  geometry  and  position 

•  simrlate  propulsion  -  determine  new  speed  from  command,  state,  and  attitude 

•  simulate  steering  -  determine  angular  velocity  from  steering  and  speed 

•  simulate  position  -  dead  reckon  from  linear  and  angular  velocity  and  time  step 


A  mechanization  diagram  is  shown  below  for  these  equations: 


terrain 


a. 


P 


- ►  Suspensio  r 

►  Piopulsio  k 

HEBBEBfjiiiB 

Figure  5  -  Feedforward  Simulator 


In  the  figure,  (x,  y,  z)  represents  the  position  of  the  vehicle  in  the  navigation  frame,  (0,  <|),  t|r) 
represents  its  attitude  in  terms  of  pitch,  roll,  and  yaw  angles  respectively,  V  is  the  vehicle  speed 
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along  the  body  y  axis,  a  is  the  steer  angle  of  the  front  wheels,  P  is  the  angular  velocity  of  the  body 
projected  onto  the  body  z  axis,  and  the  subscripts  c  and  a  represent  commanded  and  actual 
quantities  respectively. 

L2  RgftrenCT  Points 

The  positions  of  distinguished  points  on  the  body,  called  reference  points,  are  maintained  in 
navigation  coordinates  throughout  the  simulation.  The  kinematics  transforms  involved  in  doing 
this  are  documented  elsewhere  [26].  These  points  are  at  the  same  elevation  as  the  bottom  of  the 
wheels  and  are  positioned  at  the  wheel  contact  points  and  the  center  of  the  axles.  The  processing 
of  the  reference  points  proceeds  as  follows  for  each  cycle  of  the  simulation: 

•  convert  coordinates  from  body  to  nav  frame  based  on  the  vehicle  position  and  attitude 


All  aspects  of  both  system  state  and  hazardous  conditions  can  be  computed  from  the  reference 
points  and  the  terrain  elevation  under  them.  In  this  way,  coordinate  system  transformation 
operations  are  reduced  to  an  absolute  minimum. 

7^  Suspension  Model 

Using  the  reference  points  only,  a  proper  model  would: 

•  compute  the  elevation  “under”  each  wheel 

•  compute  the  deflections  of  each  wheel  from  the  minimum  potential  energy  principle 

•  compute  the  positions  of  three  points  on  the  body 

•  compute  the  body  attitude  from  these  three  points 

which  is  a  lot  of  somewhat  costly  work  to  do.  Therefore,  the  suspension  model  is  based  on: 

•  a  rigid  terrain  assumption 

•  a  rigid  suspension  assumption 

•  a  locally  planar  terrain  assumption 
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The  first  assumption  results  from  using  the  terrain  map  elevations  measured  by  a  sensor  when  the 
vehicle  was  not  loading  the  terrain.  The  second  occurs  because  the  positions  of  the  wheels  relative 
to  the  body  are  taken  as  fixed.  The  third  occurs  because  the  elevations  under  the  four  wheels  are 
used  in  forming  two  vectors  even  though  they  do  not  necessarily  lie  on  the  same  plane. 

The  last  problem  in  the  process  can  be  expressed  as  that  of  recovering  an  unknown  rotation  matrix 
from  a  few  points  which  are  transformed  using  it.  The  inverse  RPY  transform  was  computed  in 
[26]  for  this  purpose.  The  displacement  transform  from  body  coordinates  to  world  coordinates  for 
a  z-x-y  Euler  angle  sequence  can  be  expressed  as: _ 


■ 

m 

■ 

(c\|rc<j>  -  s\|/s0s({>)  -s\i/c0 

(C\)/S(]>  +  St|/S0C<)>) 
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Yaw  and  pitch  can  be  determined  from  any  vector  known  to  be  aligned  with  the  body  y  axis: 

\|r  =  atan2(r22,-^i2^ 

0  =  atanl 

Roll  can  be  derived  from  the  world  coordinates  of  a  vector  known  to  be  aligned  with  the  body  x 
axis. 

<|)  =  aran2(s0[-rjjSV  +  r2jCVl  -r3iC0,  (rjjCV  +  rjjSV)) 


The  above  equations  are  an  exact  inverse  kinematic  solution.  However,  the  system  makes  a  valid 
small  pitch  assumption  currently  in  order  to  reduce  the  trigonometric  computations.  The  vector* 
are  formed  from  the  reference  points.  The  algorithm  is  simply: 

/* 

**  Simulate  Suspension 
*/ 

zleft  =  (lf[z]  +  lr[z])  /  2.0; 

zrght  =  {rf[z]  +  rr[z])  /  2.0; 

zfrnt  =  (rf[z]  +  lf[z])  /  2.0; 

zrear  =  (rr[z]  +  Irtz])  /  2.0; 

z  =  zrear  +  tire_radius; 

pitch  =  atan2 (zfrnt  -  zrear) /wheelbase; 

roll  =  atan2{ zleft  -  zrght) /width; 

Using  a  linear  deflection  model,  it  can  be  shown  that,  since  strain  energy  is  squared  in  strain,  the 
minimum  potential  energy  principle  of  solid  mechanics  implies  that  a  least  squares  solution  to  the 
deflections  of  the  four  wheels  of  an  independent  suspension  is  also  the  solid  mechanics  solution. 
The  simple  suspension  model  seems  to  work  well  enough  as  it  is  so  a  more  refined  model  was  not 
implemented.  Such  a  model  however,  would  permit  the  representation  of  suspension  overextension 
as  a  hazard.  Also,  the  deviation  of  the  four  wheel  contact  points  from  a  plane  is  an  equivalent  model 
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under  a  rigid  suspension  assumption  and  this  would  be  simpler  to  implement. 

TA  Propiiiswn  Miidgi 

Propulsion  is  currently  modelled  as  a  proportional  controller  with  gravity  compensation^.  Under 
an  assumption  that  the  torque  at  the  wheels  can  be  directly  actuated  and  that  the  terrain  can  generate 
any  necessary  shear  reaction  (no  slip)  Newton’s  law  can  be  written  along  the  body  y  axis  as 
follows: 


The  inverse  of  the  proportional  gain  Kp  is  the  time  constant.  A  time  constant  of  5  seconds 
corresponds  to  the  generation  of  0. 1  g  command  for  a  5  m/s  speed  error.  A  limiter  is  added  to  reflect 
the  fact  that  the  output  torque  has  some  limit.  One  way  to  compute  this  limit  is  to  choose  a  pitch 
angle  corresponding  to  some  grade  that  is  the  highest  grade  that  the  vehicle  can  climb.  Under  this 
model,  the  computer  algorithm  is  the  finite  difference  version  of  the  system  differential  equation. 


5.  This  model  is  similar  to  the  JPL  speed  controller  except  that  throttle  is  actuated  and  not  torque. 

6.  If,  like  most  of  us,  the  reader  has  forgotten  Laplace  Transforms,  1/s  is  an  integral  and  s  is  a  derivative. 
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which  is  simply^: _ 

/* 

**  simulate  Propulsion 
*/ 

now  =  read_clock{ ) ; 

vdelay  =  queue_lookup(prop_queue,now  -  prop_delay) ; 

verr  =  vdelay  -  vres; 

acmd  =  verr  /  prop_time_constant; 

if  (£abs{acmd)  >  eunzuc)  acmd  =  amzuc  *  acmd  /  £abs(acmd); 
ares  =  acmd  -  g  *  sin (pitch); 
vres  +=  ares  *  dt; 


This  model  has  several  advantages  over  an  ideal  one.  It  will  correctly  cause  the  simulated  vehicle 
to  take  several  seconds  to  respond  to  a  step  input  command  and  it  will  even  cause  the  simulated 
vehicle  to  back  up  on  a  steep  grade  and  generally  slow  down  or  speed  up  depending  on  the  grade. 


7^  Steering  Model 


The  steering  model  is  involved  because  it  must  account  for  the  nonlinear  relationship  between 
curvature  and  steer  angle.  This  model  is  also  a  primary  area  of  coupling  between  speed  and  attitude 
rate,  so  it  is  one  of  the  most  important  elements  of  the  model.  The  bicycle  model  of  the  steering 
kinematics  is  given  in  [26].  Typically,  the  steering  wheel  itself  is  actuated,  so  the  loop  must  be 
closed  back  at  the  steer  angle.  This  assumes  only  that  the  steering  wheel  to  steer  angle  transform 
is  linear.  Either  position  or  speed  could  be  the  controlled  variable,  but  a  proportional  position  loop 
is  assumed®.  Both  the  position  and  the  speed  are  limited^.  The  complete  dynamics  can  be  expressed 


7.  If  we  distinguish  control  algorithms  from  complete  controllers,  many  actuator  control  algorithms  can  be 
inq>lemented  in  just  a  few  lines  of  code.  The  above  situation  is  the  rule,  not  the  exception.  Code  is  provided 
in  order  to  illustrate  that  however  sophisticated  dynamic  models  may  sound,  implementing  them  is  easy. 

8.  At  this  moment,  a  position  loop  is  used  because  a  speed  loop  requires  speed  feedbadc  which  is  not  available 
on  some  vehicles.  It  could  be  generated  internally  by  differentiating  the  position  feedback,  but  the  speed  limit 
is  considered  to  be  the  overriding  element  of  the  delay  anyway. 

9.  There  may  also  be  a  tttqiezoidal  command  velocity  profile.  This  amounts  to  an  acceleration  limit  on  the 
command. 
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the  coding  is  straightforward: 


Again 


**  simulate  Steering  Dyn2unics 
*/ 

now  =  read_clock( ) ; 

alpdelay  =  queue_lookup(steer_queue,now  -  steer_delay) ; 
alperr  =  alpdelay  -  alpres; 
if  (fabs (alperr/dt)  >  alpdotmeoc) 
alperr  =  alpdotm^LX  *  dt  *  alperr  /  fabs  (alperr)  ; 
ares  +=  alperr; 


These  few  lines  of  code  make  an  enormous  difference  in  the  high  speed  stability  of  the  system. 

2^  Dead  Reckoning 

The  3D  dead  reckoning  equations  also  form  the  system  model  in  the  Kalman  filter  which  is 
documented  elsewhere  [30].  The  attitude  rate  about  the  body  y  axis  is  available  from  the 
instantaneous  velocity  and  the  instantaneous  steer  angle  (or  curvature)  as  follows: 


11  Sensor  Head  Model 

A  controllable  pan/tilt  sensor  head  is  modelled  by  two  differential  equations  which  are  similar  to 
those  used  for  the  other  actuators.  It  is  likely  that  the  system  would  not  have  access  to  speed 
feedback,  so  again,  a  position  loop  is  used  to  model  the  dynamics.  A  loop  which  is  identical  to  the 
steering  model  can  be  used  to  reflect  the  limited  angles  of  excursion  and  the  limited  maximum 
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8.  Tactical  Planning 

The  system  adopts  the  basic  assumption  that  all  hazardous  conditions  can  be  computed  from  the 
reference  points.  Hazards  are  represented  as  sampled  time  signals  which  are  parameterized  by  the 
commands  to  which  they  correspond.  At  any  point  in  forward  projected  time,  the  system  is 
designed  to  be  robust  to  the  absence  of  information  because  the  sampling  and  the  oodusion 
problems  are  intrinsic  and  inevitable. 

M  Temporal  State  Interpolation 

Field  testing  of  the  system  on  typical  rough  terrain  indicates  that,  if  occlusions  are  distinguished 
from  poorly  sampled  regions  of  the  environment,  and  the  need  for  sophisticated  terrain  map 
interpolation  is  questionable.  Specifically,  the  degree  of  undersampling  depends  on  the  local 
incidence  angle  of  pixels  to  the  terrain  and  the  need  for  interpolation  directly  implies  that  the  terrain 
is  mostly  flat  anyway.  The  reverse  argument  is  more  compelling.  If  a  small  vertical  step  in  the 
terrain  exists,  then  it  must  span  several  pixels  due  to  its  near  normal  incidence.  Therefore,  it  caimot 
be  undersampled. 

These  observations  imply  that  it  is  urmecessary  to  interpolate  an  entire  terrain  map  before  it  is  used 
for  plaiming  computations.  It  can  be  done  on  theffy,  and  more  importantly,  it  can  be  done  on  the 
vehicle  state  vector.  The  state  vector  alone  supplies  the  information  necessary  to  propagate  the 
system  state  forward  in  time,  and  the  terrain  contact  constraint  can  be  modelled  as  a  “disturbance” 
which  modifies  the  vehicle  attitude  whenever  the  terrain  under  the  wheels  is  known  (which,  in 
practice,  is  almost  always). 

The  interpolation  algorithm  used  in  the  system  is  an  interpolation  of  both  the  state  vector  and  the 
hazard  signals  in  time,  rather  than  an  interpolation  of  the  terrain  map  in  space. 

2.2  Hazard  Identification 

Three  principal  forms  of  hazards  are  incorporated: 

•  unknown  terrain 

•  collision  of  the  terrain  with  the  body 

•  static  instability 

8.2.1  Unknown  Terrain 

Unknown  map  cells  may  arise  from  undersampling  or  terrain  self  occlusion.  An  overall 
information  density  merit  is  computed  for  each  candidate  command  which  is  based  on  the  number 
of  wheel  elevations  v/hich  are  known  at  each  time  step. 

8.2.2  Terrain- Wheel  Collision 

Wheel  collision  is  computed  as  the  slope  of  the  elevations  under  each  wheel  as  the  body  is 
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propagated  forward. 


Zj-Zi-l 


merit  (i)  = 


Q 


max 


Figure  11  -  Wheel  Collision 


In  practice,  this  signal  can  be  noisy  if  the  sensor  cannot  supply  the  angular  resolution  and  relative 
range  accuracy  necessary.  At  times,  it  is  not  used  at  all. 

8.2,3  Terrain-Body  Collision 

Body  collision  is  computed  as  the  lost  volume  under  the  body  normalized  by  the  area  under  the 
body,  so  it  is  an  expression  of  the  average  height  under  the  b<^y.  The  result  is  normalized  by  the 


8.2.4  Static  Stability 

Static  stability  measures  the  proximity  of  the  vehicle  to  tipover.  The  static  stability  margin  can  be 
expressed  as  the  remaining  angle  between  the  gravity  vector  and  the  support  polygon  formed  by 
the  wheel  contact  points  (which  is  a  rectangle  in  this  case).  Graphically,  the  margin  is  the  minimum 
distance  to  any  side  of  the  support  rectangle  normalized  by  the  height  of  the  center  of  gravity  above 
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theteirain. 


The  margin  can  be  expressed  as  the  maximum  of  the  tangent  of  simple  approximations  to  roll  and 
pitch  normalized  by  some  maximum.  The  distinction  between  measurement  as  an  angle  or  its 
tangent  is  irrelevant  for  small  angles. 


One  of  the  implications  of  this  model  is  that  the  vehicle  will  turn  into  a  steep  grade  so  as  to  increase 
the  stability  margin.  The  system  actually  computes  pitch  and  roll  margins  individually,  and 
performs  tte  maximum  outside  the  hazaud  evaluator.  This  is  a  measure  used  to  permit  easy 
debugging.  The  system  can  navigate  successfully  over  smooth  terrain  based  on  this  hazard  alone. 

8^  Haard  Rcpresmtatou 

In  general,  each  hazard  is  represented  on  some  normalized  scale  where  the  maximum  value 
indicates  certainty  of  danger  and  the  minimum  value  indicates  certainty  of  safety.  A  typical  hazard 
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M  Ha^rd  Arhitratiftn 

Conceptually,  the  evaluation  of  hazards  amounts  to.  first,  tte  generation  of  a  3D  field  of  the  form 
merit(luKardtCommaiid,time)  because  safety  is  different  for  each  command,  different  for  each 
hazard,  and  a  time  signal.  The  obstacle  avoidance  hazard  arbiter  conceptually  integrates  this  3D 
field  into  a  single  decision.  The  first  step  of  the  process  is  to  collapse  tte  time  dimension.  This  is 
done  by  computing  die  signal  norm  as  follows: 

l/a 

merit  =  (merit  (i) )  “) 
i 


for  some  power  a.  In  practice,  however,  a  straightforward  maximum  was  found  to  produce 
acceptable  results.  This  step  reduces  the  decision  field  to  merit(hazard,command). 

The  next  step  is  to  coUqise  the  hazard  dimension.  Hazards  are  considered  independent  because,  for 
example,  body  collision  is  completely  irrelevant  to  static  stability.  Therefore,  a  straightforward 
maximum  applied  across  this  dimension  gives  the  worst  unsafe  condition  for  all  dimensions  of 
safety  consisted. 

The  final  step  is  to  reduce  the  command  dimension,  and  this  has  been  discussed  in  the  context  of 
joint  arbitration  of  tactical  and  strategic  votes.  However,  before  the  tactical  vote  vector  is  passed  to 
the  arbiter  concerned,  it  is  sometimes  smoothed  by  Gaussian  filtering  in  order  to  increase 
reliability.  The  idea  is  that,  because  the  environment  exhibits  a  degree  of  smoothness,  it  is 
appropriate  at  times  for  poorly  rated  trajectories  to  degrade  the  ratings  of  their  spatial  neighbors. 
In  this  manner,  the  impact  of  errors  in  both  feedback  and  actuation  will  be  less  relevant  because 
this  mechanism  will  cause  the  vehicle  to  give  obstacles  a  wide  berth.  At  times,  this  mechanism  is 
disabled  depending  on  the  spacing  of  the  candidate  trajectories  investigated. 
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9.  Sensor  Stabilization  and  Pointing 

Sensor  head  control  algorithms  are  provided  that  incorporate  both  somatic  and  enviromnental 
feedback.  These  algorithms  can  also  used  to  control  the  field  of  view  of  real  time  programmable 
sensors. 

Azimuth  Controllers 

The  pan  axis  must  move  with  the  vehicle  and  because  it  is  mounted  to  the  vehicle,  any  response  on 
this  axis  is  useful. 

The  first  azimuth  controller  is  the  steering  regulator.  This  controller  generates  pan  commands 
which  match  the  commands  to  the  steering  wheel.  It  would  have  been  possible  to  servo  to  the 
steering  feedback,  but  that  option  is  considered  to  irtcorporate  a  delay  and  provide  no  benefit  in 
return. 

The  second  azimuth  controller  is  the  detection  zone  tracker.  The  steering  feedforward  simulator 
provides  an  intelligent  basis  for  pointing  the  pan  axis  because  the  centroid  of  the  region  which  is 
reachable  by  the  vehicle  can  easily  be  computed.  This  controller  computes  the  average  angle 
subtended  at  the  initial  position  by  the  response  curve  endpoint  In  a  sentence,  this  servo  looks 
where  the  vehicle  is  going. 

9.2  Elevation  Controllers 

The  tilt  axis  must  move  against  the  vehicle  and  therefore  must  be  able  to  respond  significantly 
faster  that  the  vehicle  itself  in  order  to  be  of  any  use. 

The  first  elevation  controUer  is  the  pitch  regulator.  This  servo  drives  the  elevation  degree  of 
freedom  to  the  opposite  of  the  vehicle  pitch.  Note  that  if  the  vehicle  pitch  is  indicated  by  an  inertial 
sensor,  the  servo  gives  effective  inertial  stabilization  of  the  head  without  the  cost  of  extra 
gyroscopes. 

The  second  elevation  controller  is  the  range  window  tracker.  In  rough  terrain,  the  field  of  view 
can  only  be  pointed  by  a  loop  closed  around  the  perception  system  because  the  body  attitude  and 
terrain  rougtmess  affect  the  projection  of  the  field  of  view  onto  the  groundplane  and  only  the  high 
level  control  loop  knows  this  information. 

The  adaptive  sweep  algorithm  provides  an  excellent  measure  of  the  deviation  of  the  sensor  tilt  from 
the  ideal  as  the  average  deviation  of  the  range  window  from  the  vertical  center  of  the  image.  This 
error  signal  is  used  to  drive  the  head,  and  in  the  process,  a  rough  terrain  adaptive  sensor  head 
controller  results.  This  controller  provides  impressive  stabilization  on  rough  terrain  and  it  can  also 
drive  a  roll  axis  with  little  modification. 
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10.  Speed  Planning 

Just  like  everything  else,  the  speed  which  should  be  commanded  of  the  vehicle  depends  on  the  local 
environment  It  makes  sense,  for  instance,  for  the  vehicle  to  slow  down  when  it  encounters 
challenging  terrain  and  likewise  increase  speed  when  benign  terrain  is  encountered. 

The  motivation  for  speed  planning  is  that  there  is  reason  to  expect  that 

•  there  is  a  way  to  measure  system  robustness 

•  there  is  a  way  to  relate  robustness  to  speed  in  rough  terms 

The  principle  used  for  a  speed  planner  is  to  close  a  proportional  control  loop  around  the  average 
predicted  merit  of  all  candidate  commands  because  this  average  merit  is  low  in  dense  hazard 
enviromnents  and  high  in  regions  free  of  hazards.  This  principle  is  complemented  by  the  fact  that 
higher  speeds  tend  to  lead  to  reduced  maneuverability  and  hence  lower  reliability  for  the  same 
local  terrain.  That  is,  robustness  can  be  altered  in  a  small  region  by  adjusting  speed  alone. 

Many  aspects  of  performance  are  implicitly  figured  in  the  algorithm.  For  example,  occlusion, 
resolution,  map  quantization  noise,  etc.  ate  all  implicitly  expressed  in  the  hazard  signals,  so  the 
system  will  eventually  teach  equilibrium  with  respect  to  the  most  constraining  sensor  characteristic 
and  an  appropriate  speed  will  emerge  naturally. 

When  a  plarmer  gets  confused  it  tends  to  stay  confused  for  a  time  and  when  it  is  relatively  stable, 
it  also  tends  to  stay  stable.  This  implies  diat  there  is  a  low  frequency  component  to  system 
robustness  which  is  long  enough  in  time  constant  to  exceed  the  vehicle  response  limits.  Hence,  a 
vehicle  may  be  able  to  respond  fast  enough  to  change  things  before  its  too  late.  It  should  also  be 
noted  that  the  tactical  controller  is  designed  so  that  a  panic  stop  (slamming  on  the  brakes)  is  always 
guaranteed  to  avoid  vehicle  damage.  The  speed  planner  uses  more  graceful  measures  to  improve 
performance. 


This  loop  is  implemented  with  a  time  constant  that  is  relatively  large  so  that  the  speed  loop  will 
respond  only  to  the  low  frequencies  in  the  overall  merit  The  results  for  a  run  of  Ae  system  in  a 
dense  obstacle  field  at  high  speed  ate  indicated  below. 


The  vehicle  was  driven  in  simulation  for  500  cycles  at  a  nominal  speed  of  4.5  m/s  at  3  Hz  cycle 
time.  When  speed  planning  was  on,  the  excursion  was  703  meters  versus  687  meters  when  it  was 
off.  The  plarmer  was  considered  to  have  failed  when  the  merit  fell  below  a  threshold  of  0.4  Vfith 
speed  planning  on,  “failure”  occurred  4%  of  the  time,  whereas  it  occurred  6%  of  the  time  with 
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Speed  Planning  Results 
4.5  m/s  nominal  speed 


1.0 


Speed  Planning  Off 


Speed  Planning  On 


50  100  ISO  200  250  0 

Cycle  Number 


SO  100  150  200  250 
Cycle  Number 


Figure  16  •  Performance  of  Speed  Planning 


speed  pknnmg  ott.  More  unportant,  me  senousness  ot  me  failures  witn  p'lanmng  otl  were  much 
worse  than  those  with  planning  on.  The  system  attempts  to  maintain  a  constant  merit  of  0.4  when 
the  loop  is  closed.  The  figure  shows  that  the  deviation  from  0.4  is  much  higher  in  the  left  figure. 
The  peaks  and  valleys  are  wider  in  the  left  figure  indicating  sustained  periods  of  confusion.  In  plain 
terms  the  algorithm  makes  it  possible  to  drive  farther  more  robustly. 


An  extra  benefit  of  the  speed  planning  loop  is  that  it  will  automatically  back  the  vehicle  up  and 
drive  off  in  another  direction  when  a  panic  stop  is  issued.  The  path  planner  supports  this  as  well 
because  it  plans  correctly  for  negative  velocities,  so  backs  up  “smartly”  avoiding  obstacles  until  a 
new  alternative  presents  itself. 
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11.  High  Speed  Autonomy  as  an  Optimal  Control  Problem 

This  section  casts  the  problem  of  high  speed  autonomy  as  an  optimal  control  problem.  The  lowest 
level  element  of  the  system  is  the  control  laws.  The  control  laws  are  responsible  for  coordinated 
control  of  all  actuators.  The  control  laws  are  implemented  in  a  tightly  coupled  hierarchy.  At  this 
level  of  conceptualization,  the  system  can  be  viewed  as  a  multi-input,  multi-output,  feedforward 
state  space  controller. 

114  Stety  Spacg  Controller 

For  a  Unear  systern^^,  the  conventional  state  space  model  of  a  system  is  the  venerated  two  matrix 
equations: _ 

^  =  Ax  +  Bu 

y  =  Cx  +  Dii 


Note  in  particular  that  the  first  equation  is  a  differential  one.  The  command  vector  ji  includes 
vehicle  steering  and  speed  as  well  as  demand  signals  to  any  sensor  heads.  The  state  vector  x 
includes  the  vehicle  steering,  speed,  and  the  state  of  motion  of  the  vehicle  body  and  the  sensor 
heads^^  The  output  vector  y  can  be  any  function  of  both  state  and  inputs,  but  in  our  case,  it  can 
be  considered  to  be  the  state  itself  (so  C  is  the  identity  and  D  is  zero).  These  equations  are  often 
indicated  in  a  block  diagram  as  shown  below: 


Figure  17  -  Linear  System  Block  Diagram 


In  this  model  so  far,  the  system  transition  matrix  of  the  last  section  can  be  identified  directly  with 
the  A  matrix  and  the  equation  which  employs  it  to  propagate  the  system  state  forward  in  time  is 
the  integrator.  The  transition  matrix  includes  the  steering  model  and  the  propulsion  model.  The  B 
matrix  can  be  identified  with  anything  which  modifies  commands  generated  by  the  system  before 
they  get  to  the  physical  hardware.  That  is,  the  B  matrix  can  be  identified  directly  with  the  delay 
queues. 


10.  Our  system  is  not  linear.  No  real  system  is  perfectly  linear.  Consider  this  a  model  for  now,  and  tbe  validity 
of  tbe  model  to  be  an  open  question. 

11.  The  state  vector  includes  more  information  tban  tbe  commands  because  tbe  number  of  state  variables  is 
dictated  by  the  order  of  the  differential  equation  being  modelled.  Tbe  system  equation  is  a  matrix  equation, 
so  its  order  is  the  dimension  of  the  A  matrix,  and  this  need  have  no  relationship  to  the  length  of  u.  This  cor¬ 
responds  to  the  fact  that  every  integral  requires  an  initial  condition.  On  a  convmitional  automobile  the  com¬ 
mand  vector  u  is  of  order  2  whoeas  the  state  vectt»'  may  have  ten  or  more  elements. 
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Ui2  Haajrd-Mfldtl 

Sometimes,  it  is  useful  to  further  distinguish  the  signals  going  to  the  plant  from  the  information 
which  is  ultimately  of  interest.  In  our  case,  the  primary  quantity  of  interest  is  vehicle  safety.  This 
can  be  indicated  with  a  reference  input  or  demand  vector  d.  Also,  there  may  be  quantities  which 
influence  the  state  of  the  system  over  which  theie  is  no  control  at  all.  Let  these  disturbances  be 
called  ]i({.  Disturbances  can  be  placed  anywhere  that  makes  sense  in  the  diagram  and  they  may  or 
may  not  be  modelled.  Further,  the  C  matrix  can  be  reintroduced  and  the  output  considered  to  be  the 
safety  of  the  vehicle  expressed  in  terms  of  the  hazard  signal  vector.  Our  system  model  has  now 
become: 


where  a  single  box  for  the  state  space  model  S  has  been  generated  by  grouping  certain  elements 
together.  Such  a  system  implemented  without  feedforward  would  be  a  regulator  which  attempts  to 
keep  the  hazard  vector  at  0. 


The  disturbances  can  be  considered  to  be  anything  that  affects  the  system  but  that  is  generated 
outside  the  system.  In  our  case,  the  requirement  that  the  vehicle  remain  on  the  terrain  surface  arises 
from  the  fact  that  the  terrain  is  incompressible  (by  assumption)  and  it  generates  forces  that  prevent 
the  vehicle  from  sinking.  Thus,  the  disturbances  can  be  identified  directly  with  the  suspension 
model.  The  ou^ut  vector  is  the  hazard  signal  vector  and  the  C  matrix  is  the  hazard  evaluation 
functions  which  map  vehicle  states  and  the  terrain  geometry  onto  estimate  of  hazards.  Notice  that 
the  hazards  are  time  signals  and  that  there  are  many  kinds.  Each  kind  of  hazard  is  one  element  of 
the  ouq>ut  vector. 

This  model  maps  very  directly  onto  the  tactical  controller.  Other  control  loops  can  be  considered 
to  be  subsumed  in  the  diagram  as  well.  Sensor  head  control  is  a  regulator  where  the  reference  input 
is  the  deviation  of  the  range  window  from  image  dead  center.  The  strategic  controller  is  a  regulator 
which  attempts  to  maintain  the  heading  error  against  the  global  path  at  zero.  A  complete  diagram 
is  far  too  detailed  to  draw  because: 
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•  the  state  vector  is  of  order  close  to  10 

•  many  of  the  matrices  are  divisible  into  blocks 

•  coordinate  system  transforms  are  complicated  and  are  a  major  source  of  coupling  and 
nonlinearity. 

113  Feedforward  Optimal  ControUcr 

Feedforward  is  employed  for  two  distinct  reasons.  First,  it  imparts  stability  to  many  of  the 
algorithms  -  particularly  the  strategic  controller  and  the  tactical  controller^^.  Second,  the  process 
by  which  safety  is  predicted  is  also  a  feedforward  process. 

The  system  attempts  to  guarantee  safety  by  predicting  into  the  future  the  relationship  between 
future  safety  and  current  command  options.  Then  it  acts  now  based  on  future  consequences.  It  does 
this  by  using  a  state  space  amulator.  This  feedforward  element  can  be  represented  by  duplicating 
the  state  space  model  and  indicating  that  it  is  an  estimate  with  a  super  hat  diacritical  mark.  The  fact 
that  an  optimal  trajectory  is  chosen  can  be  represented  by  replacing  the  summing  point  with  a  more 


The  state  space  model  maps  easily  onto  the  tactical  controller.  The  tactical  controller  is  a  kind  of 
suboptimal  controller  which  attempts  to  minimis  the  norm}^  of  the  hazard  vector.  This  is 
guaranteed  safety  again  in  contrr’.s  language. 


12.  It  bas  not  been  mentioned  yet,  but  feedforward  prevrats  “wafRing”  in  the  steering  arbitrator  because  once 
a  slight  turn  is  commwded  once,  it  affects  all  downstream  computations  by  biasing  all  subsequent  decisions 
to  reflect  the  distance  the  steering  wheel  turns  for  that  cycle.  Es^h  cycle’s  output  is  a  partial  commitment  so 
the  tactical  controller  can  only  sit  on  the  fence  with  respect  to  a  particular  obstacle  for  one  cycle.  Feedforward 
leads  to  a  more  confident  controller  because  it  recognizes  when  a  partial  conunitment  has  been  made  to  turn 
in  a  certain  direction. 

13.  The  norm  is  computed  by  integrating  first  over  time  and  second  over  the  dimensions  of  hazard  space. 
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Formally,  the  system  is  a  suboptimal  solution  to  the  following  optimal  control  problem: 


min[L(y(t))]  = 

subject  to: 

r“ 

$ 

X 

[t 

Jyi(0^ 
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t  =  A2t  + 

=  Cx  +  l 
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LNo  J 
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Du 

where  A  models  the  steering  and  propulsion  dynamics,  B  models  the  communications  delays,  and 
C  models  the  terrain  contact  constraint  as  well  as  the  mapping  from  system  state  onto  hazards.  The 
rms  time  integral  of  the  hazards  is  generalized  to  the  signal  a  norm  given  by: 

'Vo  ' 

t 

0 

TTo 

and  the  vector  length  is  generalized  to  an  a  norm  over  hazard  space: 

LK(y)=> 

l/a 

The  '‘'>'Tnal  solution  tc  "ui  optimal  control  problem  is  obtained  from  the  Pontriagin  Manmum 
Pn*  (of  whic "  : ;  nore  familiar  calculus  of  variations  is  a  special  case).  In  general,  the 

opu/f:  jntrol  prollim  generates  a  set  of  simultaneous  partial  differential  equations  with 

boundary  conditions  which  are  very  difficult  to  solve  in  real  time. 

The  system  model  amounts  to  a  complicated  differential  equation  constraint.  The  satisfaction  of 
this  constraint  is  generally  very  difficult  to  achieve.  The  set  of  trajectories  which  satisfies  the 
system  model  equations  and  which  maintain  the  vehicle  in  contact  with  rigid  terrain  is  called,  in 
optimization  lingo,  the  feasible  set 

The  system  satisfies  this  constraint  by  construction  through  feedforward.  The  inverse  system 
model  in  the  previous  diagram  is  never  evaluated  explicitly  -  the  system  simply  remembers  the 
correspondence  of  commands  to  response  trajectories  and  inverts  this  list  of  ordered  pairs.  In  this 
manner,  the  system  plans  in  actuation  space. 

The  system  solves  the  optimization  problem,  effectively,  by  sampling  the  feasible  set  of  trajectories 
at  some  practical  resolution  that  ensures  adequate  coverage  of  the  set,  and  then  by  choosing  the 
trajectory  with  the  best  value  of  the  functional  [L  (y  (t) )  ] . 
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12.  Discrete  Time  Nonlinear  Model 


The  last  section  gave  a  linear  systems  model  of  a  MIMO  system.  The  system  model  actually  used 
is  a  nonlinear  model,  but  it  can  still  be  expressed  as  a  set  of  matrix  equations  if  the  matrices 
themselves  are  allowed  to  vary  with  the  state  vector. 

12,1  State.  Vfirtttr 

The  state  vector  includes  the  position  and  attitude  of  the  body  and  the  linear  velocity  along  body 
y  and  the  angular  velocity  around  body  z. 


12,2  Command-yjBCtor 

The  conamand  vector  includes  the  commanded  speed  and  commanded  “yawrate” 


12.3  System  Model 

The  system  model  is  based  on  a  low  dynamics  a^umption.  This  is  the  assumption  that  velocity 
can  be  considered  constant  for  a  small  period  of  time.  The  model  is  given  below: 
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The  terms  in  the  bottom  of  the  vector  account  for  the  nonlinear  dependence  of  the  attitude  rate  on 
the  attitude  of  the  body.  Angular  velocity  kinematics  are  given  in  [26].  This  model  will  correctly 
account  for  motion  out  of  the  plane  even  so  far  as  to  correctly  turn  the  vehicle  around  the  body  z 
axis  instead  of  the  gravity  vector.  This  aspect  of  the  model  can  be  expressed  as  a  nonlinear 
transition  matrix,  if  desired. 
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The  relationship  between  the  yawrate  and  the  steer  angle  is: 


p  =  kV  = 


tana 

IT 


V 


If  we  ignore  the  nonlinear  limiters  for  the  purpose  of  illustration,  and  model  the  actuator  loops  as 
first  order  systems,  the  relationship  between  the  command  vector  and  the  state  derivative  vector 
can  be  written  as: 


Under  a  small  pitch  assumption,  the  terrain  contact  constraint  alters  the  attitude  each  cycle,  it  can 
be  written  as: 


The  hazard  vector  can  be  written  as  a  4  vector  which  includes  the  wheel  collision,  body  collision, 
and  stability  margin: 


The  wheel  collision  output  relationships  are: 
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T^e  static  stability  output  relationships  are: 


12.7  Implementation  in  Special  Purpose  Hardware 


The  entile  system  including  perception,  planning,  and  control,  runs  in  less  than  50  msecs  on  a 
SPARC  10  platform  and  this  is  already  a  very  high  cycle  rate  for  the  entire  perceive-think-act  loop. 
However,  higher  performance  can  be  achieved  by  porting  the  system  to  special  purpose  hardware. 
In  principle,  the  con^l  laws  are  implementable  solely  in  terms  of  matrix  algebra,  trigonometric 
functions  and  a  few  signal  operators  -  because  they  really  are  just  one  big  differential  equation.  For 
example,  a  graphics  rendering  pipeline  would  perform  all  of  the  necessary  geometric 
transformations  on  image  sensory  data  to  generate  the  map  data  structure.  The  system  model  is 
Uter^y  a  discrete  matrix  differentia  equation,  and  the  hazard  model  can  be  implemented  as  a 
matrix.  TTie  arbitrator  could  then  be  implemented  in  a  digital  signal  processor. 
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